#pragma once
#ifndef PRM_H
#define PRM_H

#include "basic_typedef.h"
#include "Graph.h"
#include "CollisionDetector.h"
#include "Sampler.h"
#include "Kd_tree.h"

// Should be used with the Graph wrapper
struct Less_than_int
{
  bool operator()(const int& p1, 
                  const int& p2) const 
  {
    return (p1 < p2);
  }
};

#define EPS 0.01

class Prm
{
public:
	/*
		IMPORTANT:	The main code of the PRM algorithm should be placed in this file.
					You may change its structure, but make sure you that it remains consistent
					with the run() operation in Planner.h
	*/


	Prm (	int number_vertices, 
			int k_nearest, 
			CollisionDetector* col, 
			Sampler* sampler,
			Point_2 start, Point_2 target) : 
		m_number_vertices(number_vertices), m_k_nearest(k_nearest), 
		  m_col(col), m_sampler(sampler),m_graph(0),
		  m_start(start), m_target(target)
	{
    m_graph = new Graph<int, Less_than_int>(0, true);
    m_kd_tree = new Kd_tree_2<Kernel>;
	}

	~Prm()
	{
    delete m_graph;
		delete m_kd_tree;
	}
	
	//	This operation is supposed to generate the roadmap (sample configurations
	//	and connect them.
	//	TODO: implement it.
	void			generate_roadmap() {
    m_points.push_back(m_start);
    m_points.push_back(m_target);
    m_graph->add_vertex(0);
    m_graph->add_vertex(1);
    for (size_t i = 0; i < m_number_vertices; ++i) {
      m_points.push_back(m_sampler->generate_sample());
      m_graph->add_vertex(i + 2);
    }
    m_kd_tree->insert(m_points.begin(), m_points.end());
    for (size_t i = 0; i < m_points.size(); ++i) {
      vector<Point_2> nn;
      m_kd_tree->k_nearest_neighbors(m_points[i], m_k_nearest, std::back_inserter(nn));
      for (vector<Point_2>::const_iterator iter = nn.begin(); iter != nn.end(); ++iter) {
        if (m_col->local_planner(m_points[i], *iter, EPS)) {
          int j = std::distance(m_points.begin(), std::find(m_points.begin(), m_points.end(), *iter));
          m_graph->add_edge(i, j, sqrt(squared_distance(m_points[i], *iter)));
        }
      }
    }
	}

	//	Returns a point path from start to target.
	//	If a path doesn't exist, returns empty vector.
	//	TODO: Implement this function.
	vector<Point_2>			retrieve_path() {
		vector<Point_2> retVal;
    if (!m_graph->is_in_same_cc(0, 1)) {
      return retVal;
    }
    list<int> path;
    m_graph->find_path(0, 1, path);
    for (list<int>::const_iterator iter = path.begin(); iter != path.end(); ++iter) {
      retVal.push_back(m_points[*iter]);
    }
		return retVal;
	}

private:

	int									m_number_vertices;	// number of sampled vertices
	int									m_k_nearest;		// maximal degree of vertex in roadmap
	CollisionDetector*					m_col;
	Sampler*							m_sampler;
	
	Graph<int,Less_than_int>*			m_graph;			//	Graph structure
	Kd_tree_2<Kernel>*					m_kd_tree;			//	Kd-tree for nearest neighbor search

	Point_2 							m_start,m_target;	//	Start and target configurations

  vector<Point_2> m_points;
};

#endif
